#include "bmm150.h"
#include "stm32h723xx.h"
#include "stm32h7xx_hal_def.h"
#include "stm32h7xx_hal_gpio.h"
#include <stdint.h>

#define BMM150_I2C_ADDR 0x13 // BMM150 7bit I2C地址
#define BMM150_CHIPID_REG_ADDR 0x40 // BMM150芯片ID寄存器地址
#define BMM150_CHIP_ID 0x32 // BMM150芯片ID

#define BMM150_CHANNEL_CFG_REG_ADDR 0x4E
#define BMM150_DATAX_LSB_REG_ADDR 0x42
#define BMM150_DATAX_MSB_REG_ADDR 0x43
#define BMM150_DATAY_LSB_REG_ADDR 0x44
#define BMM150_DATAY_MSB_REG_ADDR 0x45
#define BMM150_DATAZ_LSB_REG_ADDR 0x46
#define BMM150_DATAZ_MSB_REG_ADDR 0x47

#define BMM150_OPMODE_REG_ADDR 0x4C
#define BMM150_OPMODE_NORMAL 0x06

#define BMM150_POWER_CTRL_REG_ADDR 0x4B

typedef union {
    uint8_t byte;
    struct {
        uint8_t self_test : 1; // bits 0
        uint8_t opmode : 2; // bits 1~2
        uint8_t data_rate : 3; // bits 3~5
        uint8_t adv_st : 2; // bits 6~7
    };
} OpMode_Reg;

OpMode_Reg opmode_ctrl_reg;
SoftwareI2C_HandleTypeDef bmm150_hi2c = {
    .scl_port = GPIOB,
    .scl_pin = GPIO_PIN_3,
    .sda_port = GPIOB,
    .sda_pin = GPIO_PIN_4,
    .speed = 100000,
    .delay_us = 5
};


/**
 * 1、bmm150上电后，先进入suspend模式（此时只能操作0x4B寄存器），最后一位置0也可进入suspend模式
 * 2、通过把0x4B寄存器最后一位置1进入sleep模式（此时可以操作所有寄存器）
 * 3、
 */
HAL_StatusTypeDef BMM150_Init(void)
{
    uint8_t data = 0x01; // 设置为正常工作模式
    HAL_StatusTypeDef rslt;
    SoftI2C_Init(&bmm150_hi2c);

    rslt = SoftI2C_Mem_Write(&bmm150_hi2c, BMM150_I2C_ADDR, BMM150_POWER_CTRL_REG_ADDR, I2C_MEMADD_SIZE_8BIT, &data, 1);
    if (rslt != HAL_OK)
        return HAL_ERROR;

    opmode_ctrl_reg.opmode = 0x00; // 00: normal, 01: force, 10: not use, 11: sleep mode
    opmode_ctrl_reg.data_rate = 0x00; // 00: 10hz(normal模式有效)
    rslt = SoftI2C_Mem_Write(&bmm150_hi2c, BMM150_I2C_ADDR, BMM150_OPMODE_REG_ADDR, I2C_MEMADD_SIZE_8BIT, (uint8_t*)&opmode_ctrl_reg, 1);
    if (rslt != HAL_OK)
        return HAL_ERROR;

    return HAL_OK;
}

uint8_t BMM150_ReadChipID(void)
{
    uint8_t data = 0xff;
    SoftI2C_Mem_Read(&bmm150_hi2c, BMM150_I2C_ADDR, BMM150_CHIPID_REG_ADDR, I2C_MEMADD_SIZE_8BIT, &data, 1);
    return data;
}

uint8_t _BMM150_EnterForceMode(void)
{
    opmode_ctrl_reg.opmode = 0x01;
    uint8_t rslt = SoftI2C_Mem_Write(&bmm150_hi2c, BMM150_I2C_ADDR, BMM150_OPMODE_REG_ADDR, I2C_MEMADD_SIZE_8BIT, (uint8_t*)&opmode_ctrl_reg, 1);
    if (rslt != HAL_OK)
        return HAL_ERROR;
    return HAL_OK;
}

uint8_t _BMM150_EnterNormalMode(void)
{
    opmode_ctrl_reg.opmode = 0x00;
    uint8_t rslt = SoftI2C_Mem_Write(&bmm150_hi2c, BMM150_I2C_ADDR, BMM150_OPMODE_REG_ADDR, I2C_MEMADD_SIZE_8BIT, (uint8_t*)&opmode_ctrl_reg, 1);
    if (rslt != HAL_OK)
        return HAL_ERROR;
    return HAL_OK;
}

HAL_StatusTypeDef BMM150_MagDataGet(int16_t* x, int16_t* y, int16_t* z)
{
    // _BMM150_EnterForceMode(); // 进入force模式
    // HAL_Delay(10); // 等待测量完成
    // 3. 验证模式已自动返回Sleep（OP_MODE=0x03=11）
    // if (SoftI2C_Mem_Read(&bmm150_hi2c, BMM150_I2C_ADDR, BMM150_OPMODE_REG_ADDR, I2C_MEMADD_SIZE_8BIT, (uint8_t*)&opmode_ctrl_reg, 1) != HAL_OK)
    //     return HAL_ERROR;
    // if (opmode_ctrl_reg.opmode != 0x03) // 11: sleep mode
    //     return opmode_ctrl_reg.byte; // 测量未完成或失败

    uint8_t data_buf[8] = { 0 };
    // 从datax_lsb开始读取8个寄存器
    SoftI2C_Mem_Read(&bmm150_hi2c, BMM150_I2C_ADDR, BMM150_DATAX_LSB_REG_ADDR, I2C_MEMADD_SIZE_8BIT, data_buf, 8);
    // 5. 拼接原始数据（按数据手册5.1节格式）
    // X轴：13位（DATAX_L高5位 + DATAX_H 8位，bit[0~11]数据位，bit[12]符号位）
    *x = (((uint16_t)data_buf[1] << 8) | (data_buf[0])) >> 3;
    if (*x & 0x1000) {
        *x |= 0xE000; // 符号扩展
    }
    *y = (((uint16_t)data_buf[3] << 8) | (data_buf[2])) >> 3;
    if (*y & 0x1000) {
        *y |= 0xE000; // 符号扩展
    }
    *z = (((uint16_t)data_buf[5] << 8) | (data_buf[4])) >> 1;
    if (*z & 0x4000) {
        *z |= 0x8000; // 符号扩展
    }
    return HAL_OK;
}
